/**
 * @file smt_vel_curve.c
 * @author mjsz (1349513606@qq.com)
 * @brief 速度曲线优化算法实现文件
 * @version 0.1
 * @date 2025-05-23
 * 
 * @copyright Copyright (c) 2025
 * 
 */
#include "smt.h"

#define MAX_FREQ 4000000

/**
 * @brief 快速计算e的x次幂
 *
 * 该函数通过位操作和浮点运算结合的方式，实现快速计算e的x次幂的功能。
 *
 * @param x 浮点数，表示e的指数
 *
 * @return 返回e的x次幂的结果，类型为float
 */
inline float fast_expf(float x)
{
    union
    {
        uint32_t i;
        float f;
    } v;
    v.i = (1 << 23) * (1.4426950409f * x + 126.93490512f);
    return v.f;
}

/**
 * @brief 优化加速度时间函数
 *
 * 根据初始速度v0，目标速度v1，以及步数steps，计算每步的速度和对应的预分频值，并将结果存储在speed_buf和psc_buf中。
 *
 * @param v0 初始速度
 * @param v1 目标速度
 * @param steps 总步数
 * @param speed_buf 存储每步速度的缓冲区
 * @param psc_buf 存储每步预分频值的缓冲区
 */
void OptimizeAccelTime(int v0, int v1, int steps,
                       uint32_t *speed_buf, uint16_t *psc_buf)
{
    if (steps <= 0 || (!speed_buf && !psc_buf)) return;

    const float k = 8.0f;
    const float half_steps = steps / 2.0f;

#ifdef MT_SD_OUT_SPDTAB
    if (speed_buf) speed_buf[0] = steps;
#endif
    if (psc_buf) psc_buf[0] = steps;

    for (int i = 1; i <= steps; i++)
    {
        float t = (i - half_steps) / half_steps;
        float sigmoid = 1.0f / (1.0f + fast_expf(-k * t));
        uint32_t speed = v0 + (v1 - v0) * sigmoid;
        if (speed == 0) speed = 1; // 防止除零

        int idx = steps - i + 1; // 反置写入

#ifdef MT_SD_OUT_SPDTAB
        if (speed_buf) speed_buf[idx] = speed;
#endif
        if (psc_buf) {
            uint16_t psc = (uint16_t)(MAX_FREQ / speed);
            psc = (psc > 0xFFFF) ? 0xFFFF : psc;
            psc_buf[idx] = psc;
        }
    }
}


/**
 * @brief 计算步进电机的速度曲线
 *
 * 根据运动参数和目标位置，计算步进电机的速度曲线，并存储在指定的速度曲线结构体中。
 *
 * @param motion_params 运动参数结构体，包含加速度段步数、减速段步数、起始速度、最大速度和结束速度。
 * @param curve 存储速度曲线的结构体，包括加速段、减速段和匀速段的步数以及相应的速度数组。
 * @param target_pos 目标位置，即电机需要移动到的总步数。
 */
void stm_vel_curve_cacl(smt_motion_params_t motion_params, smt_vel_curve_t curve, int target_pos)
{
    int acc_steps = motion_params->acc_steps;
    int dec_steps = motion_params->dec_steps;
    int total_steps = target_pos;
    int uniform_steps = total_steps - acc_steps - dec_steps;
    if (uniform_steps < 0) uniform_steps = 0;

    // 假设 acc_tab/dec_tab 是 uint16_t*，需要在外部分配好内存
    // 生成加速段
    if (acc_steps > 0 && curve->acc_tab) {
        OptimizeAccelTime(motion_params->velstart, motion_params->velmax, acc_steps, NULL, curve->acc_tab);
    }
    // 生成减速段
    if (dec_steps > 0 && curve->dec_tab) {
        OptimizeAccelTime(motion_params->velmax, motion_params->velend, dec_steps, NULL, curve->dec_tab);
    }
    // 匀速段步数
    curve->acc_steps = acc_steps;
    curve->dec_steps = dec_steps;
    curve->uniform_steps = uniform_steps;
}
